//
// Created by jimmyteng on 19-6-8.
//

#include "visual_compass/visual_compass.h"
#include <opencv2/opencv.hpp>
#include <zconf.h>

int VisualCompass::Initialize(const cv::Mat &K, const cv::Mat &DistCoef, const cv::Mat &R) {
  if (K.empty() || DistCoef.empty()) {
    return ERROR_INPUT_DATA_ERROR;
  }
  K_ = K.clone();
  DistCoef_ = DistCoef.clone();
  K_.at<float>(0, 2) = K_.at<float>(0, 2)/* - width_offset_*/;
  K_.at<float>(1, 2) = K_.at<float>(1, 2)/* - height_offset_*/;
  cv::invert(R, R_);
  //generate map for distortion
  cv::fisheye::initUndistortRectifyMap(K_, DistCoef_, R_,
                                       K_, cv::Size(640, 480), CV_32FC1, mUndistMap1_, mUndistMap2_);
  auto theta_delta = CV_PI / 180;
  for (int index_theta = 0; index_theta < (number_angle_ << 1); ++index_theta) {
    sin_[index_theta] = sin(theta_delta * index_theta);
    cos_[index_theta] = cos(theta_delta * index_theta);
  }
  init_flg_ = true;
  std::cout << "Inited!" << std::endl;
  return NO_ERROR;
}

int VisualCompass::Detect(const cv::Mat &image, double &degree, double &Confidence) {
  if (init_flg_) {
    if (image.empty() || image.rows != height_ || image.cols != width_) {
      return ERROR_INPUT_DATA_ERROR;
    }
    memset(accumulative_table_, 0, sizeof(accumulative_table_));
    memset(res, 0, sizeof(res));
    _ImagePreprocess(image);
    _DetectOrientation(degree, Confidence);
    return NO_ERROR;
  } else {
    std::cout << "UnInited!" << std::endl;
    return ERROR_UNINITIALIZE;
  }
}

int VisualCompass::_ImagePreprocess(const cv::Mat &image) {
  try {
    remap(image, image, mUndistMap1_, mUndistMap2_, CV_INTER_LINEAR);
    cv::Mat imageROI = image(cv::Rect(width_offset_, height_offset_, image.cols - (width_offset_ << 1),
                                      image.rows - (height_offset_ << 1)));

    cv::Mat distort = cv::Mat::zeros(imageROI.rows * 2, imageROI.cols * 2, CV_8U);
    cv::Mat image_big_window_blur = cv::Mat::zeros(imageROI.rows, imageROI.cols, CV_32FC1);
    cv::Mat image_small_window_blur = cv::Mat::zeros(imageROI.rows, imageROI.cols, CV_32FC1);
    cv::blur(imageROI, image_big_window_blur, cv::Size(big_window_size_, big_window_size_));
    cv::blur(imageROI, image_small_window_blur, cv::Size(small_window_size_, small_window_size_));
    diff_image_ = image_small_window_blur - image_big_window_blur;
    diff_image_ = cv::abs(diff_image_);
    cv::threshold(diff_image_, binary_image_, detection_threthold_, UCHAR_MAX, cv::THRESH_BINARY);
  }
  catch (cv::Exception &e) {
    std::cout << e.what() << std::endl;
  }

  return NO_ERROR;
}

int VisualCompass::_DetectOrientation(double &degree, double &confidence) {
  for (size_t index_col = 0; index_col < binary_image_.cols; ++index_col) {
    for (size_t index_row = 0; index_row < binary_image_.rows; ++index_row) {
      for (size_t index_theta = 0; index_theta < number_angle_; ++index_theta) {
        auto value = binary_image_.at<uchar>(index_row, index_col);
        if (value == UCHAR_MAX) {
          int p = std::round(static_cast<double>(index_col) * cos_[index_theta] +
              static_cast<double>(index_row) * sin_[index_theta] + std::max(roi_width_, roi_height_));
          accumulative_table_[index_theta][p]++;
          p = std::round(static_cast<double>(index_col) * cos_[index_theta + 90] +
              static_cast<double>(index_row) * sin_[index_theta + 90] + std::max(roi_width_, roi_height_));
          accumulative_table_[index_theta][p]++;
        }
      }
    }
  }
  int center_x = 240;//roi_width_ / 2;
  int center_y = 240;//roi_height_ / 2;
  for (size_t index_theta = 0; index_theta < number_angle_ << 1; ++index_theta) {

    int p1 = std::round(static_cast<double>(center_x - sin_[index_theta] * r_) * cos_[index_theta] +
        static_cast<double>(center_y + cos_[index_theta] * r_) * sin_[index_theta] + std::max(roi_width_, roi_height_));
    int p2 = std::round(static_cast<double>(center_x + sin_[index_theta] * r_) * cos_[index_theta] +
        static_cast<double>(center_y - cos_[index_theta] * r_) * sin_[index_theta]
                            + std::max(roi_width_, roi_height_));
    std::cout << "center_x = " << center_x << "; center_y = " << center_y << std::endl;
    std::cout << "p1 = " << p1 << "; p2 = " << p2 << std::endl;
    for (int index_rho = std::min(p1, p2); index_rho <= std::max(p1, p2); ++index_rho) {
      accumulative_table_[index_theta][index_rho] = 0;
    }
  }

  for (size_t index_theta = 0;
       index_theta < number_angle_;
       ++index_theta) {
    for (size_t index_rho = 0;
         index_rho < number_rho_;
         ++index_rho) {
      res[index_theta] += accumulative_table_[index_theta][index_rho] * accumulative_table_[index_theta][index_rho]
          * accumulative_table_[index_theta][index_rho];
    }
  }
  int index = 0;
  double value = -DBL_MAX;
  double sum = 0.0;
  for (
      int index_theta = 0;
      index_theta < number_angle_;
      ++index_theta) {
    sum += res[index_theta];
    if (value < res[index_theta]) {
      index = index_theta;
      value = res[index_theta];
    }
  }
  degree = index;
  confidence = value / sum * 100;
  if (confidence > confidence_threthold_)
    return NO_ERROR;
  else {
    return ERROR_RUN_FAIL;
  }
}
